
import rospy
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry
# from math import pi
from ros.navigation import *
import time
<<<<<<< HEAD
# pi原本为3.14……， 但由于存在实际误差，旋转180度需要 pi*16/15, 故重定义pi =3.3510321638291
pi = 3.3510321638291

class Control:

    def __init__(self):
        self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5)
        self.rate = 50
        self.r = rospy.Rate(self.rate)

    def forward(self, dis):
        goal_distance = dis
        linear_speed = 0.2

        move_cmd = Twist()
        if(dis < 0):
            linear_speed = linear_speed*(-1)
        move_cmd.linear.x = linear_speed

        linear_duration = goal_distance/linear_speed
        ticks = int(linear_duration*self.rate)
        for t in range(ticks):
            # one node can publish msgs to different topics, here only publish
            # to /cmd_vel
            self.cmd_vel.publish(move_cmd)
            self.r.sleep()  # sleep according to the rate
        self.cmd_vel.publish(Twist())

    def rotate(self, angle):
        goal_angular = angle
        angular_speed = 0.5
        mvoe_cmd = Twist()
        if(angle < 0):
            angular_speed *= -1
        mvoe_cmd.angular.z = angular_speed

        angular_duration = goal_angular/angular_speed
        ticks = int(angular_duration*self.rate)
        for t in range(ticks):
            self.cmd_vel.publish(mvoe_cmd)
            self.r.sleep()
        self.cmd_vel.publish(Twist())
=======
from Control import *
import sys
sys.path.append('..')
from pydobot.dobot import Dobot
# 获取图片
# from cv_2022 import *
# 文字识别
from ocr.detect_test import *
# from slide import *
from threading import Thread
# from control2 import Control2
from get_laser import Obstacle, reverse_adopt_forward_route
# from get_laser2 import adopt_forward
from single_laser import *
import argparse


def get_odom():
    while(1):
        # (topic,topic_type,timeout)
        var = rospy.wait_for_message('/odom', Odometry, timeout=5)
        x= var.pose.pose.position.x
        y= var.pose.pose.position.y
        angle = var.pose.pose.orientation.x
        # x= format(var.pose.pose.position.x, '.4f')
        # y=  format(var.pose.pose.position.y, '.4f')
        print("/odom: " + format(x, '.4f') , format(y, '.4f'),format(angle,'.6f'))
>>>>>>> origin/develop

def get_odom():
#   while(1):
      # (topic,topic_type,timeout)
    var = rospy.wait_for_message('/odom', Odometry, timeout=5)
    x= var.pose.pose.position.x
    y= var.pose.pose.position.y
    # x= format(var.pose.pose.position.x, '.4f')
    # y=  format(var.pose.pose.position.y, '.4f')
    print("/odom: " + format(x, '.4f') , format(y, '.4f'))

    rospy.sleep(1.)
    return x,y

def build_graph():
    na = Navigation()
    na.send_goal("start")
    # 左边
    na.send_position([3.8, 0.4],-90)
    rospy.sleep(1)
    na.send_position([3.8, 0.4],90)

    # 中间
    na.send_position([1.7, 0.4],0)
    na.send_position([1.7, 0.4],-90)
    rospy.sleep(1)
    na.send_position([1.7, 0.4],180)
    
    # 右侧 起点附近
    na.send_position([-0.30, 0.4],-90)
    na.send_position([-0.30, 0.4],180)
    rospy.sleep(1)
    na.send_position([-0.30, 0.4],90)

    na.send_goal("start")

    """
    3.846,0.142       -0.617,0.070
    3.884,1.022       -0.525,1.176
    """


def rotate_test():
    na = Navigation()
    Control().forward(-0.5)
    # na.send_goal("start")
    # na.send_position([1.573,0.9],0)
    x,y = get_odom()
    na.send_position([x,y],0)
    na.send_position([x,y],90)
    na.send_position([x,y],180)
def main_process():
    start_time = time.time()
    # Control().forward(3.7)
    # Control().rotate(-pi)

    angle1 = 90
    angle2 = -90
    # 位置顺序 自右向左
    position_list = [ [-0.283,0.900, angle2], # 0 右下角
                      [-0.283,-0.210, angle1], [0.363,-0.210,angle1],  # 右上角
                      [ 1.357,-0.205, angle1], [1.993,-0.205,angle1],  # 中间
                      [ 2.971,-0.205, angle1], [3.611,-0.210,angle1],   # 左边
                      [ 3.611,0.900, angle2] ]  # 左下角
    platform_center = [1.573,0.9,angle2]
    na = Navigation()
    # Control().forward(0.5)
    na.send_goal("start")

    for point in position_list:
        na.send_position((platform_center[0],platform_center[1]),platform_center[2])
        na.send_position( (point[0],point[1]) ,point[2])

        Control().forward(0.5)

    na.send_goal("start")
    end_time  = time.time()
    print("cost time:{} s".format(end_time - start_time))
    print("cost time:{} m".format((end_time - start_time)/60)) # 5.9m
<<<<<<< HEAD
=======

def go_middle():
    na = Navigation()
    na.send_goal("middle")
def go_home():
    na = Navigation()
    na.send_goal("start")



def print_pose():
    (x, y, z, r, j1, j2, j3, j4) = device.pose()
    print(f'x:{x} y:{y} z:{z} j1:{j1} j2:{j2} j3:{j3} j4:{j4}')
def grab_action():
    global device
    while 1:
        print_pose()
    (x, y, z, r, j1, j2, j3, j4) = device.pose()
    device.move_to(70.670,-270.409,  -45.961, r, wait=True) # 移向抓取目标
    device.suck(True)  # 吸取
    time.sleep(0.5)
    device.rotate_to(-90 ,0, 0 ,0, wait=True) # 抬升
    time.sleep(0.3)
    device.rotate_to(90, 28.5, 46.0 ,0.0 , wait=True) # 回到存储区
    device.suck(False) # 气泵停止
    device.rotate_to(-90, 0,0 ,0.0 ,  wait=True) # 回到分拣区

    device.move_to( -54.988 ,-281.7512 , -45.354, r, wait=True)
    device.suck(True)
    time.sleep(0.5)
    device.rotate_to(-90 ,0, 0 ,0, wait=True) # 抬升
    time.sleep(0.3)
    device.rotate_to(90, 47.5, 65.1 ,0.0 ,  wait=True) # 放下
    device.suck(False)
    device.rotate_to(-90, 0,0 ,0.0 ,  wait=True)

    device.rotate_to(-90, 0, 60, 0, wait=True) # 机械臂初始化归位

# middle LEFT(0.564, 1.20) 
#        Right(2.798, 1.20)
def route_test():
    na = Navigation()
    co = Control()
    # na.send_goal("start")
    middle_left = 0.500
    middle_right = 2.850

    platform_center = [1.573,0.9,-90]
    cur_pos = middle_left
    while(cur_pos < middle_right):
        na.send_position([cur_pos,1.090],-90)
        time.sleep(3.5)
        break
        co.forward(1.7)
        # from_wall_dis = obstacle.get_scan()
        # rospy.loginfo("laser_dis: %f",from_wall_dis)
        # if(from_wall_dis > 0.30):
        #     adopt_forward(0.1,0.30)
        # break
        # grab_action()
        # na.send_position((platform_center[0],platform_center[1]),platform_center[2])
        cur_pos += 0.20

    # na.send_goal("start")

# 第二层抓取 8.56 , 223.491 , 5.9    89.89 , 34.47 , 45.81 , 0.0
# 第一层抓取 9 ,223.945 , -31.69    89.21 , 46.46 , 57.81,  0.0

# right 54.4, 174.6, -41.9  | 72.68, 41.36, 77.135 , 0
# left  -56.9, 176.2, -42.2 | 107.925,42.76, 76.50, 0
def drob_test():
    r = device.pose()[3]
    # catch_coordnate = [[89.89 , 34.47 , 45.81 , 0.0],[89.21 , 46.46 , 57.81,  0.0]]
    # robot 车身的存储区域
    catch_coordnate = [[65, 43, 64, 0],[110, 45, 64, 0]]
    for j1,j2,j3,j4 in catch_coordnate:
        # device.rotate_to(90, 20, 0 ,0.0 ,  wait=True)
        # 低下
        # device.move_to(x,y,z ,r, wait=True)
        device.jump_to(j1,j2,j3,j4, wait=True)
        # 吸取
        device.suck(True)
        time.sleep(0.5)
        # 抬起转向投递盒
        device.jump_to(-90, 60, 20, 0,  wait=True)
        time.sleep(0.5)
        device.suck(False)
    device.rotate_to(90, 0, 60, 0,  wait=True)


def nvigation_test():
    na = Navigation()
    co = Control()
    for i in range(8):
        na.send_goal("p"+str(i))
        adopt_forward(-0.3,0.11)
        time.sleep(3)
        co.forward(1.0)
    na.send_goal("start")




def grab_test(catch_coordnate,save_position):
    r = device.pose()[3]

    x,y,z = catch_coordnate[0],catch_coordnate[1], catch_coordnate[2]
    device.move_to(x , y , z , 0, wait=True) # 移向抓取目标
    # print_pose()
    device.suck(True)  # 吸取

    time.sleep(1.0)
    device.move_to(x ,y, z+80 ,0, wait=True) # 抬升
    time.sleep(0.3)
    if save_position == 0:
        device.jump_to(65, 22, 54, 20,  wait=True) # 回到存储区
    elif save_position == 1:
        device.jump_to(110, 22, 54, 60,  wait=True) # 回到存储区
    # device.rotate_to(90, 30, 45 ,0.0 , wait=True) # 回到存储区
    # time.sleep(1)
    device.suck(False) # 气泵停止
    time.sleep(0.5)
    # print("release:")
    # print_pose()
    device.rotate_to(90, 0, 0, 0,wait=True)
    device.rotate_to(-90, 0, 0, 0,wait=True)
    print("grab once Finish")



def grob_route_offset(arm_position,centre,target_p=1):
    if target_p == 1:
        if centre[0] < 150:
            arm_position[0] += 80
        elif centre[0] < 200:
            arm_position[0] += 60
        elif centre[0] < 400:
            arm_position[0] += 30
        elif centre[0]< 650:
             arm_position[0] += 10
        elif centre[0]< 800:
            arm_position[0] -= 30
        else:
            arm_position[0] -= 60

    elif target_p  == 0:
        arm_position[1] -= 20

        if centre[0] <700:
            arm_position[0] += 40
        if centre[0] <900:
            arm_position[0] += 20

            
    else:
        # arm_position[1] -= 20
        if centre[0] <700:
            arm_position[0] -= 30
        if centre[0] <900:
            arm_position[0] -= 15

def grob_route_limit(arm_position,target_p=1):
    if target_p == 1:
        if arm_position[0] < -160:
            arm_position[0] = -160
        elif arm_position[0] > 150:
            arm_position[0] = 150
        if arm_position[1] < -290: 
            arm_position[1] = -290

    elif target_p == 0:
        if arm_position[0] < -160:
            arm_position[0] = -160
        elif arm_position[0] > 220:
            arm_position[0] = 220
        if arm_position[1] < -230: 
            arm_position[1] = -230
    else:
        if arm_position[0] < -160:
            arm_position[0] = -160
        elif arm_position[0] > 160:
            arm_position[0] = 160
        if arm_position[1] < -230: 
            arm_position[1] = -230

# target_p = 0 1 2  Left Middle Right
def grob_route_new(cnt_centre_list,target_p=1):
    if len(cnt_centre_list )> 2:
        cnt_centre_list = cnt_centre_list[:2]
    origin_centre = [540, 360 ]
    # Lefr: x:178.83702087402344 y:-167.78375244140625 z:113.55181884765625 j1:-43.17353057861328 j2:16.389230728149414 j3:6.234043598175049 j4:0.0
    # Right: x:-119.69556427001953 y:-182.5864715576172 z:138.33045959472656 j1:-123.24705505371094 j2:4.40556001663208 j3:-1.4537315368652344 j4:0.0
    # 150 -214.42224926757814 -34
    # x:215.44717407226562 y:-234.36952209472656 z:-32.55054473876953 j1:-47.4088249206543 j2:73.05128479003906 j3:29.28477668762207 j4:0.0
    # x:-174.71926879882812 y:-245.69351196289062 z:-38.88544464111328 j1:-125.41764831542969 j2:67.48367309570312 j3:38.04008865356445 j4:0.0
    # -174.7,-245.6,-38.8
    arm_list = [[178.83, -167.78, -34],
                [-9.0675, -207.4406, -34],
                [-119.69,-182.58,-34]]
    offset_coef = 0.1736 # 每个像素代表的实际距离 mm
    for i,[centre,size] in enumerate(cnt_centre_list):
        if target_p >=0 and target_p <3:
            arm_position = arm_list[target_p] # -90 0 5 0
        else:
            return
        offset_x = (centre[0] - origin_centre[0]) * offset_coef  
        offset_y = (centre[1] - origin_centre[1]) * offset_coef  
        arm_position[0] -= offset_x
        # 40 <- RGB摄像头与吸盘x坐标偏差值补偿
        # arm_position[0] += 40
        arm_position[1] += offset_y
        # 60 <- RGB摄像头与吸盘y坐标偏差补偿
        arm_position[1] -= 70

        grob_route_offset(arm_position,centre,target_p)
        grob_route_limit(arm_position,target_p)
        # if centre[0] < 150:
        #     arm_position[0] += 80
        # elif centre[0] < 200:
        #     arm_position[0] += 60
        # elif centre[0] < 400:
        #     arm_position[0] += 30
        # elif centre[0]< 650:
        #      arm_position[0] += 10
        # elif centre[0]< 800:
        #     arm_position[0] -= 30
        # else:
        #     arm_position[0] -= 60


        print(arm_position[0], arm_position[1] ,arm_position[2])
        grab_test([arm_position[0], arm_position[1] ,arm_position[2]],i)

# 完整单次抓取流程
def grob_route(cnt_centre_list):
    origin_centre = [1080//2, 720 //2 ]
    left_line_x = 450
    right_line_x = 750
    cnt_centre_list = cnt_centre_list[:2]
    # standard_box_centre_pixel = [[234.6, 443.9],[644.5, 444.0],[1000, 444.0]]
    # standard_catch_points = [[117.837 , -265.959 ,-46.702],  #  left
    #                          [0.243  ,  -263.562 , -46.317], #  middle
    #                          [-112.238, -262.355,  -46.381]] #  right
    standard_box_centre_pixel = [[365, 450],[630.5, 450.0],[1000, 450.0]]
    standard_catch_points = [[ 106.63, -249.25, -44.25],  #  left
                             [-5.38,  -252.01, -44.08], #  middle
                             [-124.16, -249.06, -44.97]] #  right
    offset_coef_of_Pixel2Real = 3.0
    # 默认抓取左侧
    box_region = 0
    # x y 抓取坐标的补偿值
    x_offset = 0
    y_offset = 0
    for i,[centre,size] in enumerate(cnt_centre_list):
        # 每次只抓取2个盒子
        if i> 1:
            break
        print(centre)
        # 自适应移动机械臂
        # 确定抓取区域
        if centre[0] < 0 or centre[0] > 1080:
            print("centre coordnate over range 1080")
            continue
        # 左侧
        if centre[0] < left_line_x:
            print("catch LEFT")
            box_region = 0
        # 中间
        elif centre[0] < right_line_x:
            print("catch MIDDLE")
            box_region = 1
        # 右侧
        elif centre[0] >= right_line_x:
            print("catch RIGHT")
            box_region = 2
        else:
            print("centre unlimite range!")

        catch_point = standard_catch_points[box_region]
        pixel_point = standard_box_centre_pixel[box_region]
        # 坐标弥补值= （ 真实坐标  -  标准坐标 ）  *  弥补系数
        x_offset = (pixel_point[0] - centre[0]) / offset_coef_of_Pixel2Real
        y_offset = (pixel_point[1] - centre[1]) / 1

        catch_point[0] += x_offset
        catch_point[1] -= y_offset
        if catch_point[0] < -160:
            catch_point[0] = -160
        elif catch_point[0] > 150:
            catch_point[0] = 150
        if catch_point[1] < -300: 
            catch_point[1] = -280
        print(catch_point[0], catch_point[1] ,catch_point[2])
        # 注意 弥补值 的正负， 对于 x和y 是相反的。
        # x 自左向右增大，与机械臂抓取坐标相同，弥补值 取正值。
        # y 自上向下增大，与机械臂抓取坐标相反，故 弥补值 取负值
        grab_test([catch_point[0], catch_point[1] ,catch_point[2]],i)

'''
    ** drob 抓取流程动作  V1.0 ** 
'''
def drob_test(layer):
    r = device.pose()[3]
    #                       左                 右
    catch_coordnate = [[110, 45, 64, 0],[65, 43, 64, 0]]
    if layer !=0 and layer != 1:
        print("drob arm mode error")
        return
    [j1,j2,j3,j4] = catch_coordnate[layer]
    # 转到抓取区域
    device.jump_to(j1,j2,j3,j4, wait=True)
    # 吸取
    device.suck(True)
    time.sleep(0.5)
    # 抬起转向投递盒
    device.jump_to(-90, 45, 0, 0,  wait=True)
    time.sleep(0.5)
    device.suck(False)
    # device.rotate_to(90, 0, 60, 0,  wait=True)


def drob_route(cnt_centre_list):
    cnt_centre_list = cnt_centre_list[:2]
    # standard_box_centre_pixel = [[234.6, 443.9],[644.5, 444.0],[1000, 444.0]]
    # standard_catch_points = [[117.837 , -265.959 ,-46.702],  #  left
    #                          [0.243  ,  -263.562 , -46.317], #  middle
    #                          [-112.238, -262.355,  -46.381]] #  right
    middle_spilt_x = 700
    standard_box_centre_pixel = [[375, 584],[870, 540]]
    standard_catch_points = [[-63,226.26, -38.37], #  left
                             [ 96.99, 213.04, -46.25]] #  right
    offset_coef_of_Pixel2Real = 3.2
    # 默认抓取左侧
    box_region = 0
    # x y 抓取坐标的补偿值
    x_offset = 0
    y_offset = 0
    for i,[centre,size] in enumerate(cnt_centre_list):
        # 每次只抓取2个盒子
        if i> 1:
            break
        print(centre)
        # 自适应移动机械臂
        # 确定抓取区域
        if centre[0] < 0 or centre[0] > 1920:
            print("centre coordnate over range 1920")
            continue
        # 左侧
        if centre[0] < middle_spilt_x:
            print("catch LEFT")
            box_region = 0
        # 右侧
        elif centre[0] >= middle_spilt_x:
            print("catch RIGHT")
            box_region = 1
        else:
            print("centre unlimite range!")

        catch_point = standard_catch_points[box_region]
        pixel_point = standard_box_centre_pixel[box_region]
        # 坐标弥补值= （ 真实坐标  -  标准坐标 ）  *  弥补系数
        x_offset = ( centre[0] - pixel_point[0]) / offset_coef_of_Pixel2Real
        y_offset = ( centre[1] - pixel_point[1] ) / 1
        catch_point[0] += x_offset
        catch_point[1] -= y_offset
        if catch_point[0] < -160:
            catch_point[0] = -160
        elif catch_point[0] > 150:
            catch_point[0] = 150
        if catch_point[1] < -300: 
            catch_point[1] = -280
        print(catch_point[0], catch_point[1] ,catch_point[2])
    
        drob_test2([catch_point[0], catch_point[1] ,catch_point[2]])


'''
    ** drob 抓取流程动作  V2.0 ** 
'''
def drob_test2(catch_coordnate):
    r = device.pose()[3]

    x,y,z = catch_coordnate[0],catch_coordnate[1], catch_coordnate[2]
    device.move_to(x , y , z , r, wait=True) # 移向抓取目标
    rospy.sleep(0.5)
    device.suck(True)  # 吸取

    # rospy.sleep(1.0)
    device.move_to(x ,y, z+80 ,r, wait=True) # 抬升
    # rospy.sleep(0.3)
    device.rotate_to(-90, 45, 0, 0,  wait=True)
    # rospy.sleep(0.5)
    device.suck(False) # 气泵停止
    device.rotate_to(90, 0, 0, 0,wait=True)
    print("drob once Finish")

def drob_route2(cnt_centre_list,layer):
    standard_box_centre_pixel = [[437, 520],[861, 520]]

    standard_catch_points = [[73.69, 217.54, -41.67], #  left
                             [ -55.85, 217.93, -41.40]] #  right     
    offset_coef_of_Pixel2Real = 3.2
    catch_point = standard_catch_points[layer]
    pixel_point = standard_box_centre_pixel[layer]
    # 确保列表下标未超出
    if cnt_centre_list[layer:] !=[] :
        # 检测当前点位与标准点位是否对应，避免左侧的抓取点对应右侧的识别点
        if abs(cnt_centre_list[layer][0][0] - pixel_point[0]) < 250:
            # cnt_centre_list=[ position, area_size ]
            centre = cnt_centre_list[layer][0]
            # 坐标弥补值= （ 真实坐标  -  标准坐标 ）  *  弥补系数
            x_offset = (pixel_point[0] - centre[0]) / offset_coef_of_Pixel2Real
            y_offset = (pixel_point[1] - centre[1]) / offset_coef_of_Pixel2Real
            # x_offset = ( centre[0] - pixel_point[0]) / offset_coef_of_Pixel2Real
            # y_offset = ( centre[1] - pixel_point[1] ) / 1
            catch_point[0] += x_offset
            catch_point[1] -= y_offset
            if catch_point[0] < -160:
                catch_point[0] = -160
            elif catch_point[0] > 140:
                catch_point[0] = 140
            if catch_point[1] < -300: 
                catch_point[1] = -280
            elif catch_point[1] > 230: 
                catch_point[1] = 230
            print("Drob catch by Adaptation: {}".format(catch_point))
            # print(catch_point[0], catch_point[1] ,catch_point[2])
        # else:
            # LOGS_INFO("catch_point not match pixel_point ")
            
    else:
        print("Drob catch by Origin: {}".format(catch_point))
    drob_test2([catch_point[0], catch_point[1] ,catch_point[2]])

'''
    ** drob 抓取流程动作  V3.0 ** 
'''
def drob_route3(cnt_centre_list,layer):
    grob_position=[[86.68, 178.83, -35.0],
                   [-71.16, 193.01,-35.0]]

    drob_test2(grob_position[layer])


def deliver_check(layer):
    deliver_img = pre_take_pic()
    img,cnt_centre_list,boxes_list,extra_big_box = detect_prepare(deliver_img,1)
    LOGS_INFO("deliver_check cnt_centre_list:{}".format(cnt_centre_list))
    if len(cnt_centre_list) < 1:
        LOGS_INFO("Deliver Check Clear!")
        return
    if layer >=0 and layer < 2:
        drob_test(layer)
        LOGS_INFO("deliver repeat {}".format(layer))
    else:
        LOGS_INFO("layer over range!")

def take_pic_for_define(mode=0):
    # Lefr: x:178.83702087402344 y:-167.78375244140625 z:113.55181884765625 j1:-43.17353057861328 j2:16.389230728149414 j3:6.234043598175049 j4:0.0
    # Right: x:-119.69556427001953 y:-182.5864715576172 z:138.33045959472656 j1:-123.24705505371094 j2:4.40556001663208 j3:-1.4537315368652344 j4:0.0
    if mode == 0:
        #grab
        device.rotate_to(-90,0,5,0, True)
        # device.rotate_to(-43.17, 16.38, 6.2,0, True)
        # device.rotate_to(-123.24, 4.40, -1.45,0, True)
    elif mode == 1:
        # drob
        device.rotate_to(90,0,5,0, True)

    box_color_img = pre_take_pic()
    if mode == 1:
        box_color_img[0:300, 0:pipeline_w] = 0
    cv2.imwrite("/home/tdb2/2022_jsjds/detection_2022/test/img/0.jpg",box_color_img)
    print(box_color_img.shape)
    if box_color_img is not None:
        img,cnt_centre_list,boxes_list,extra_big_box= detect_prepare(box_color_img,1)
        # cnt_centre_list 再排序
        # _,detect_word_list = Final_Match_Detect(img,extra_big_box,"g0")
        # print(detect_word_list)
        print(cnt_centre_list)
        '''
        ['河南', '湖南']
        centre: (234.6, 443.9)                 117.837 , -265.959 ,-43.702  |   -66.09, 65.173 , 43.07 , 0.0
        centre: (644.5, 444.0)   COEF:3.485    0.243  ,  -263.562 , -42.317 |   -89.94, 56.69 , 52.38 , 0.0
        不准确的右侧中心centre: (984.5, 430.0)     准确的抓取点 -112.238, -262.355,  -42.381 |   -113.16, 62.95 , 44.90, 0.0
        '''

        '''
        centre: (390, 379)       106.63 -249.25, -53.25 | -66.83, 62.37, 52.00, 0.0
        centre: (740, 445)       -15.38,  -252.01, -51.08 | -93.49, 57.09, 57.89, 0.0
                                 -124.16, -249.06, -48.97 | -116.49, 62.93, 48.66, 0.0
        '''
        if mode == 0:
            # grob_route(cnt_centre_list)
            grob_route_new(cnt_centre_list,1)
        elif mode == 1:
            # drob_route2(cnt_centre_list,0)
            # drob_route2(cnt_centre_list,1)
            drob_route3(cnt_centre_list,0)
            drob_route3(cnt_centre_list,1)
            # device.rotate_to(90,0,5,0, True)
            # deliver_check(0)
    else:
        print("img is None!")



# 单独测试得出的点位并不能在正式场景中使用偏差过大
def grab_points_test():
    # 0.483 ,1.167      0.651, 0.186       0.875 ,1.204    1.128,.157    1.530,1.157
    # 1.745,1.176       1.978,1.176         2.240,1.194    2.511,1.138,  2.763,1.138
    # grab_points = [[0.490,1.090], [0.710,1.09],[0.958,1.090],[1.199,1.085],[1.449,1.077],   # 0 1 2 3 4 
    #            [1.715,1.085],[2.02, 1.087],[2.252,1.080],[2.577, 1.075], [2.785,1.110]]  # 5 6 7 8 9
    # grab_points = [[0.485,1.100], [0.710,1.10],[0.958,1.100],[1.199,1.095],[1.449,1.087],   # 0 1 2 3 4 
    #            [1.715,1.095],[2.070, 1.097],[2.302,1.100],[2.607, 1.090], [2.815,1.100]]  # 5 6 7 8 9
    # grab_points = [[0.500,0.990], [0.750,0.990],[0.945,0.990],[1.199,0.990],[1.465,0.990],   # 0 1 2 3 4 
            #    [1.700,0.990],[2.015, 0.990],[2.262,0.990],[2.560, 0.990], [2.785,0.990]]  # 5 6 7 8 9
    # grab_points = [[0.500,0.990], [0.750,0.990],[1.080,0.990],[1.300,0.990],[1.520,0.990],   # 0 1 2 3 4 
    #            [1.730,0.990],[1.975, 0.990],[2.350,0.990],[2.600, 0.990], [2.850,0.990]]  # 5 6 7 8 9
    # grab_points = [[0.500,0.990], [0.780,0.990],[1.080,0.990],[1.300,0.990],[1.530,0.990],   # 0 1 2 3 4 
    #            [1.780,0.990],[2.075, 0.990],[2.380,0.990],[2.650, 0.990], [2.905,0.990]]  # 5 6 7 8 9
    grab_points = [[0.480,0.970], [0.720,0.970],[0.99,0.970],[1.265,0.970],[1.510,0.970],   # 0 1 2 3 4 
               [1.800,0.970],[2.080, 0.970],[2.305,0.970],[2.620, 0.970], [2.865,0.970]]  # 5 6 7 8 9
    navigation = Navigation()
    co = Control() 
    p = 0       
    for [cur_pos,middle_y] in grab_points:
        navigation.send_position([cur_pos,middle_y],-90)
        adopt_forward(-0.30,0.12)
        # rospy.sleep(1)
        rospy.loginfo("Point: {}".format(p))
        # reverse_adopt_forward_route(p)
        p+=1
        # co.forward(0.45)
        # from_wall_dis = obstacle.get_scan()
        # rospy.loginfo("laser_dis: %f",from_wall_dis)
        # if(from_wall_dis > 0.30):
        #     adopt_forward(0.10,0.30)
        # device.rotate_to(-90,0,5,0,wait=True)
        # break
        # time.sleep(0.3)
        # pre_take_pic()
        rospy.sleep(3)
        co.forward(1.1)
        # rospy.sleep(1)

def get_odom_test(co):
    while 1:
        print(co.get_odom())
        rospy.sleep(1)


def mini_move():
    laser_dis = obstacle.get_scan()
    rospy.loginfo("laser_dis: %f",laser_dis)
    if(laser_dis > 0.30):
        adopt_forward(0.10,0.30)
        rospy.loginfo("laser_dis: %f",laser_dis)




>>>>>>> origin/develop
'''
x <-----| (0,0)
        |
        |
        |
        v y
以小车初始点为（0，0）

|  -90度    ^  90度      0度 
|           |           <-------
|           |
v           |

'''
<<<<<<< HEAD
if __name__ == "__main__":

    # main_process()
    rospy.init_node("forward_back", anonymous=False)
    # main_process()
    build_graph()
    # rotate_test()

    # na = Navigation()
    # na.send_goal("start")
=======
# 0.426, 1.115


# 5.46349283059438m
if __name__ == "__main__":

    parser = argparse.ArgumentParser(description='test')
    parser.add_argument('--ros',action='store_true',default=True, help='Init ROS node')
    parser.add_argument('--gohome',action='store_true',default=False,help='Robot go home')    
    parser.add_argument("--grab",action='store_true',default=False,help='Dobot arm grab test')
    parser.add_argument("--route",action='store_true',default=False,help='Robot move points test')
    args = parser.parse_args()

    rospy.init_node("basic_control", anonymous=False)      
    
    start_time = time.time()
    obstacle = Obstacle()  
    # co = Control()
    # # co.forward(-1.0)
    port = "/dev/dobot_arm"
    device = Dobot(port=port, verbose=False)
    device.suck(False)
    device._set_queued_cmd_clear()
    device.clear_all_alarms_state()
    # device.go_home()
    device.rotate_to(-90,0,5,0,wait=True)

    subscribe_main_thread = Thread(target=subscribe_main,args=[])
    subscribe_main_thread.start()

    # device.rotate_to(90,0,5,0,wait=True)
    # co = Control2()
    # [point, angle]
    # point  .x .y .z 
    # angle  -3.14 ~ 3.14
#   -3.14 ^ 3.14
#         |
# -1.5<---| ---> 1.5
#         |
#         v 0
    # text = co.get_odom()
    
    # device.rotate_to(-90, 0, 30, 0, wait=True) # 机械臂初始化归位
    # get_odom_thread = Thread(target=get_odom_test,args=[co])
    # get_odom_thread.start()

    # get_odom()
    # rotate_test()

    # grab_test()  # 移动 抓取 测试
    # mini_move()
    # device.rotate_to(-90,0,5,0,wait=True)
    # grab_action()
    # reverse_adopt_forward_route(1)
    # grab_points_test()
    # nvigation_test()
    # take_pic_for_define(0)
    # take_pic_for_define(1)
    # build_graph()
    go_home()
    # adopt_forward(-0.3,0.12)
    # 目标点移动测试
    if args.route:
        route_test()
    # go_home() #  回到出发区
    if args.gohome:
        go_home()

    # drob_test()

    # co.slide(0.5)
    
    # 左右    前后      上下
    # 92.216 -252.384 -44.323        -69.75  59.68  51.50  0.0
    # -24.832  -249.178  -43.069     -95.69  53.72  56.75  0.0
    # -133.175 -248.739   -44.45  -118.16 62.61 46.45  0.0
    # grab_action()  # 机械臂测试
    # drob_test()
    # nvigation_test() # 导航测试 

    # route_test() # 分拣台点位
    # take_pic_for_define()

    # grab_test([124.522, -291.936 , -46.745])
    # grab_test([16.208, -284.100 , -51.000])
    # grab_test([-93.578 , -289.544 , -45.979])
    # grab_test([92.216, -272.384, -47.323])
    # grab_test([-24.832,  -272.178 , -47.069])
    # grab_test([-133.175, -272.739 ,  -47.45])

    end_time = time.time()
    cost_time = end_time - start_time
    print("cost time:{}s".format(cost_time ))
    print("cost time:{}m".format(cost_time /60))
>>>>>>> origin/develop
